/****************************************************************************
 *
 *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
#include <unistd.h>
#include "DriverFramework.hpp"
#include "PCA9685.hpp"
#include <math.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>

using namespace DriverFramework;

class PressureTester
{
public:
    static const int TEST_PASS = 0;
    static const int TEST_FAIL = 1;

    PressureTester()
    {}

    int run(void);

private:
    int         m_pass;
    bool		m_done = false;
};

static int _writeReg(int m_fd, uint8_t address, uint8_t *in_buffer, int length) {
    if (m_fd == 0) {
        DF_LOG_ERR("error: i2c bus is not yet opened");
        return -1;
    }

    uint8_t write_buffer[length + 1];

    if (in_buffer) {
        memcpy(&write_buffer[1], in_buffer, length);
    }

    /* Save the address of the register to read from in the write buffer for the combined write. */
    write_buffer[0] = address;

    int bytes_written = ::write(m_fd, (char *) write_buffer, length + 1);

    if (bytes_written == length + 1) {
        return 0;
    } else {
        DF_LOG_ERR("Error: i2c write failed. Reported %d bytes written",
               bytes_written);

        return -1;
    }
}

static void rush() {

    int m_fd = ::open("/dev/i2c-1", O_RDWR);

    if (m_fd < 0) {
        DF_LOG_ERR("Error: I2CDevObj::init failed on ::open()");
        return;
    }

    ioctl(m_fd, I2C_SLAVE, 0x40);

    //reset all
    uint8_t resetData[4] = {0x00, 0x00, 0x00, 0x00};
    uint8_t cmd[1];

    _writeReg(m_fd, PCA9685_RA_ALL_LED_ON_L, resetData, 4);

    /* Wait for the last pulse to end */
    usleep(2);

    //set freq
    /* Shutdown before sleeping.
     * see p.14 of PCA9685 product datasheet
     */
    cmd[0] = PCA9685_ALL_LED_OFF_H_SHUT;
    _writeReg(m_fd, PCA9685_RA_ALL_LED_OFF_H, cmd, 1);

    /* Put PCA9685 to sleep (required to write prescaler) */
    cmd[0] = PCA9685_MODE1_SLEEP_BIT;
    _writeReg(m_fd, PCA9685_RA_MODE1, cmd, 1);

    /* Calculate prescale and save frequency using this value: it may be
     * different from @freq_hz due to rounding/ceiling. We use ceil() rather
     * than round() so the resulting frequency is never greater than @freq_hz
     */
    uint8_t prescale = ceil(PCA9685_EXTERNAL_CLOCK / (4096 * 50)) - 1;
    uint16_t m_frequency = PCA9685_EXTERNAL_CLOCK / (4096 * (prescale + 1));

    /* Write prescale value to match frequency */
    cmd[0] = prescale;
    _writeReg(m_fd, PCA9685_RA_PRE_SCALE, cmd, 1);

    /* Enable external clocking */
    cmd[0] = PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT;
    _writeReg(m_fd, PCA9685_RA_MODE1, cmd, 1);

    /* Restart the device to apply new settings and enable auto-incremented write */
    cmd[0] = PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT;
    _writeReg(m_fd, PCA9685_RA_MODE1, cmd, 1);

    /*
     * scratch buffer size is always for all the channels, but we write only
     * from min_ch to max_ch
     */
    uint8_t data[13 * 4];

    for (int k = 0; ; k++) {
        for (unsigned ch = 0; ch < 13; ch++) {
            uint16_t period_us = 2000;
            uint16_t length = 0;

            length = round((period_us * 4096) / (1000000.f / m_frequency)) - 1;

            data[ch * 4 + 0] = 0;
            data[ch * 4 + 1] = 0;
            data[ch * 4 + 2] = length & 0xFF;
            data[ch * 4 + 3] = length >> 8;
        }

        //is kernel will care i2c lock?
        _writeReg(m_fd, PCA9685_RA_LED0_ON_L + 4 * (3 + 0),
                  &data[0], 13 * 4);

        usleep(100000);
    }
    close(m_fd);

}

int PressureTester::run()
{
    DF_LOG_INFO("Entering: run");
    // Default is fail unless pass critera met
    m_pass = TEST_FAIL;


    rush();

//    PCA9685 pwm("/dev", 0x40, true, 27, 3);

//    pwm.init();

//    for (int i =0; i<100; i++) {
//        int j = i % 5;

//        pwm.write(j, 1500);
//        usleep(100000);
//    }

    m_pass = TEST_PASS;

    DF_LOG_INFO("Closing pressure sensor\n");

    return m_pass;
}

int do_test();
int do_test()
{
    int ret = Framework::initialize();

    if (ret < 0) {
        return ret;
    }

    PressureTester pt;

    ret = pt.run();

    Framework::shutdown();

    DF_LOG_INFO("Test %s", (ret == PressureTester::TEST_PASS) ? "PASSED" : "FAILED");
    return ret;
}

